#include "wontec_485.h"
#include <stdio.h>
#include <string.h>


void shortToHexStr(unsigned short dataIn,unsigned char * dataOut)
{
    int i=0;
    unsigned char tmpData = 0;
    for(i=0;i<4;i++)
    {
        tmpData = (dataIn>>(12-i*4))&0x000f;
        //printf("i=%d,tmpData=%d\n",i,tmpData);
        if(tmpData>=0 && tmpData<=9)
        {
            dataOut[i] = '0' + tmpData;
        }
        else if(tmpData>=0x0a && tmpData<=0x0f) 
        {
            dataOut[i] = 'A' + (tmpData - 10);
        }
    }
}

void hexStrToShort(unsigned char * dataIn,unsigned short * dataOut)
{
    int i=0;
    unsigned short tmpDataOut = 0;
    unsigned char tmpDataIn = 0;

    for(i=0;i<4;i++)
    {
        tmpDataIn = dataIn[i];//(dataIn[i]<<(12-4*i));

        if(tmpDataIn>='a' && tmpDataIn<='f')
        {
            tmpDataIn = tmpDataIn - 'a' + 10;
        }

        if(tmpDataIn>='A' && tmpDataIn<='F')
        {
            tmpDataIn = tmpDataIn - 'A' + 10;
        }

        if(tmpDataIn>='0' && tmpDataIn<='9')
        {
            tmpDataIn -= '0';
        }
        tmpDataOut += (tmpDataIn<<(12-4*i));
    }

    *dataOut = tmpDataOut;
}

unsigned short cal_crc(unsigned char *byte, unsigned char nbyte){
    unsigned short itemp=0xFFFF;
    unsigned char i;
    while(nbyte--)
    {
        itemp ^= *byte;
        byte++;
        for (i=0; i<8; i++)
        {
            if (itemp& 0x1)
            {
                itemp>>= 1;
                itemp ^= 0xA001;
            }else
            {
                itemp>>= 1;
            }
        }
    }
    return itemp;
}

int wontec_frame_serialize(wontec_485_frame * pstFrameIn,
                 unsigned char * dataBufOut, unsigned char *dataLenOut)
{
    unsigned char len_crc = 0;
    unsigned short crcValue = 0xffff;
    unsigned char * ptrBuffOut = dataBufOut;
    if(pstFrameIn==NULL || dataBufOut==NULL)
    {
        *dataLenOut = 0;
        return 1;
    }
    *ptrBuffOut++ = FRAME_START_CODE;
    if(pstFrameIn->func_code == 0x46)
    {
        *ptrBuffOut++ = pstFrameIn->func_code;
        len_crc = 2;
    }
    else 
    {
        *ptrBuffOut++ = pstFrameIn->slave_addrH;
        *ptrBuffOut++ = pstFrameIn->slave_addrL;
        *ptrBuffOut++ = pstFrameIn->func_code;
        len_crc = 4;  
    }
    
    if(   (pstFrameIn->dataLen>0) 
      &&  (pstFrameIn->dataLen < (MAX_WT_BUFF_LEN-10))
      &&  (pstFrameIn->data!=NULL) )
    {
        memcpy(ptrBuffOut,pstFrameIn->data,pstFrameIn->dataLen);
        len_crc+=pstFrameIn->dataLen;
        ptrBuffOut+=pstFrameIn->dataLen;
    }
    // else
    // {
    //     *dataLenOut = 0;
    //     //return 2;
    // }
    

    crcValue = cal_crc(dataBufOut,len_crc);

    //printf("crc: %04x\n",crcValue);

    shortToHexStr(crcValue,ptrBuffOut);
    ptrBuffOut+=4;
    *ptrBuffOut++ = ((FRAME_END_CODE&0xff00)>>8);
    *ptrBuffOut++ = (FRAME_END_CODE&0x00ff);

    //len_crc+=4;
    *dataLenOut = len_crc+6;
    return 0;

}

int wontec_frame_decode(unsigned char * dataBufIn, unsigned char dataLenIn,
                        wontec_485_frame * pstFrameOut){
    
    unsigned char * ptrBuff = dataBufIn;
    unsigned char curPos = 0;
    unsigned short crcValInframe;
    unsigned short crcValCalc;
    if(dataLenIn>MAX_WT_BUFF_LEN || dataLenIn<8)
    {
        printf("len[%d] is invalied\n",dataLenIn);
        return 1;
    }

    if(dataBufIn==NULL)
    {
        return 2;
    }

    if(FRAME_START_CODE != *ptrBuff++)
    {
        return 3;
    }
    curPos ++;
    pstFrameOut->slave_addrH = *ptrBuff++;
    pstFrameOut->slave_addrL = *ptrBuff++;

    pstFrameOut->slave_addrH -= '0';
    pstFrameOut->slave_addrL -= '0';
    curPos += 2;
    pstFrameOut->func_code = *ptrBuff++;
    curPos++;
    pstFrameOut->dataLen = dataLenIn - 10;

    //memcpy(pstFrameOut->data,ptrBuff,pstFrameOut->dataLen);
    pstFrameOut->data = ptrBuff;
    curPos += pstFrameOut->dataLen;
    ptrBuff+= pstFrameOut->dataLen;

    crcValCalc = cal_crc(dataBufIn,dataLenIn-6);

    hexStrToShort(ptrBuff,&crcValInframe);

    if(crcValCalc!=crcValInframe)
    {
        printf("crc in frame:%04x, crc calced:%04x\n",crcValInframe,crcValCalc);
        return 4;
    }
    ptrBuff+=4;
    curPos+=4;

    if(*ptrBuff++ != 0x0D)
    {
      return 5; 
    }

    if(*ptrBuff!=0x0A)
    {
        return 6;
    }

    return 0;
}
